#include "SpeedThread.h"
#define ColorArraySize 3


static void SpeedtimerCallback( xTimerHandle pxTimer );

Comms_Msg_Struct *Msg;
Comms_Msg_Struct *curSpeed;
Comms_Msg_Struct cmsSpeed;
Comms_Msg_Struct *outMsg;
Comms_Msg_Struct cmsOut;

xTimerHandle Speedtimer;
xQueueHandle SpeedThreadQueue;

extern xQueueHandle DataManagerThreadQueue;
extern xQueueHandle CommandThreadQueue;
extern xQueueHandle WebQueue;
extern unsigned int checkStart;
unsigned char ColorHistory[ColorArraySize];

static unsigned int timeCount = 0;
unsigned int topSpeed = 0;

void SpeedThreadTaskCode( void * pvParameters )
{
	unsigned int lowTH = 150;
	unsigned int highTH = 999;
	unsigned int stopTH = 15;
	
	
	curSpeed = &cmsSpeed;
	outMsg = &cmsOut;
	int i;
	// initialize color array 
	for(i=0;i<ColorArraySize;i++)
		ColorHistory[i] = i*15;

	cmsSpeed.msgType = Speed;
	cmsSpeed.counter = 0x00;
	cmsSpeed.size = 0x03;
	for(i = 0; i < 6; i++)
		cmsSpeed.data[i] = 0;
		
	cmsOut.msgType = 0x06;
	cmsOut.counter = 0x00;
	cmsOut.size = 0x01;
	for(i = 0; i < 6; i++)
		cmsOut.data[i] = 0;

	 Speedtimer = xTimerCreate
	(  /* Just a text name, not used by the RTOS kernel. */
		(const signed char*)"Speedtimer",
		/* The timer period in ticks. */
		( 250 / portTICK_RATE_MS  ),
		/* The timers will auto-reload themselves when they expire. */
		pdTRUE,
		/* Assign each timer a unique id equal to its array index. */
		( void * ) 3,
		/* Each timer calls the same callback when it expires. */
		SpeedtimerCallback
	);
	
	if( xTimerStart( Speedtimer, 0 ) != pdPASS )
	{
	/* The timer could not be set into the Active state. */				 
	}

	for(;;)
	{	
		if( SpeedThreadQueue != 0 )
		{
			if( xQueueReceive( SpeedThreadQueue, (void*)&( Msg ), ( portTickType ) 0 ) ) 
			{ 
				if(Msg->msgType == DATAMANAGER)
				{
					// if data received is color data
					if( Msg->size == 1 )
					{								  
						AddColorToHistory(Msg->data[0]);
	
						if( DebounceColor( ColorHistory, ColorArraySize, lowTH ) )
						{
						 	cmsSpeed.data[0] = Low;	
						}
						else if( DebounceColor( ColorHistory, ColorArraySize, highTH ) )
						{
						 	cmsSpeed.data[0] = High;	
						}
						else if( DebounceColor( ColorHistory, ColorArraySize, stopTH ) )
						{
						 	cmsSpeed.data[0] = Finish;	
							cmsOut.data[0] = Finish;
							checkStart = 0;
							if( DataManagerThreadQueue != 0 )
							{
								if(xQueueSend( DataManagerThreadQueue, (void * ) &outMsg, ( portTickType ) 0 ) != pdPASS){};
							}
						} 
						else
						{
							cmsSpeed.data[0] = None;	
						}
					}
					// if data received was encoder data
					else if ( Msg->size == 2 )
					{
					 	//speed in cm/s
						int distance = Msg->data[0];
						distance = (distance<<8) + Msg->data[1];
						
						int speed = 4*(distance/timeCount);
						cmsSpeed.data[1] = (unsigned char) (speed & 0xFF);
						if(speed > topSpeed)
						{
							topSpeed = speed;
						}
												
						timeCount = 0;

						cmsSpeed.data[2] = topSpeed;
						cmsSpeed.data[3] = Msg->data[2];
						cmsSpeed.data[4] = Msg->data[3];
							
					} 
				}
			
				if( CommandThreadQueue != 0 )
				{
					if(xQueueSend( CommandThreadQueue, (void * ) &curSpeed, ( portTickType ) 0 ) != pdPASS){};
				}
				if( WebQueue != 0 )
				{
					if(xQueueSend( WebQueue, (void * ) &curSpeed, ( portTickType ) 0 ) != pdPASS){};
				}
			}		
		}
	}
}

static void SpeedtimerCallback( xTimerHandle pxTimer )
{	    	
	//every 0.1 s, timeCount will increment
	timeCount++;
}

void Init_SpeedThread()
{
	xTaskHandle SpeedThreadHandle;	
	/* Create the task, storing the handle.  Note that the passed parameter 
	ucParameterToPass must exist for the lifetime of the task, so in this 
	case is declared static.  If it was just an an automatic stack variable 
	it might no longer exist, or at least have been corrupted, by the time
	the new task attempts to access it. */
	xTaskCreate( SpeedThreadTaskCode, ( signed char * ) "Speed Thread Task", 2*configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY
	, &SpeedThreadHandle );
	SpeedThreadQueue = xQueueCreate( 10, sizeof(Comms_Msg_Struct*)  );
}

void AddColorToHistory(unsigned char msg){
        unsigned char temp, temp2;
		int i;			
		for(i=0;i<(ColorArraySize-2);i++){
        temp = ColorHistory[i];
		temp2 = ColorHistory[i+1];
		ColorHistory[i+1] = temp;
        ColorHistory[i+2] = temp2;
		}
		ColorHistory[0] = msg;
}

unsigned char DebounceColor(unsigned char * ArrayPtr, unsigned char size, int Threshold){
 	int i,flag;
	flag = 1;
	for(i=0;i<size-1;i++)
	{
		if((abs(ArrayPtr[i] - ArrayPtr[i+1]) < 30))
		 	continue;
		else
			flag=0;	
	}
	if(flag==1)
	{
		if((ArrayPtr[0]) < (Threshold+15) && (ArrayPtr[0]) > (Threshold-15))
	  		return 1;
	}
		return 0;
			
}